# 卡尔曼滤波

# 卡尔曼增益k(卡尔曼核心公式) = (上一次估计误差)/(上一次估计误差 + 本次测量误差)
# 本次估计值 = 上一次的估计值 + 系数k(gain) * (当前测量值(Zk) - 上一次的估计值hat(x)k-1)
# 新的估计误差 = (1 - k) * 上一次估计误差

   
class KalmanFilter:
    # detectorData：探测数据
    def __init__(self,detectorData: list):
        Zk = [103,99,97,104,102,105,95,96,97,103,104,100,101,103,96,99,98,102]  #模拟真实测量的数据
        Emea = 5.0         #测量误差,同一工具进行测量这个值不会变
        Eesti = 3.0        #估计误差,根据经验给定
        K = 0.0            #卡尔曼增益  K ∈ [0,1]
        Hat_X = 103.0       #估计值,随意给定
        for i in range(1,18,1):        #从1开始计算
            K = Eesti/(Emea + Eesti)   #更新卡尔曼增益
            Hat_X = Hat_X + K * (Zk[i] - Hat_X)  #更新估计值
            Eesti = (1.0 - K) * Eesti            #更新估计误差
            print(f"第{i}次 ","实际测量数据:{:.3f}".format(Zk[i])," 增益:{:.3f}".format(K)," 估计值:{:.3f}".format(Hat_X))
